#pragma once

#include <AP_AHRS/AP_AHRS.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_Buffer/AP_Buffer.h>                  // FIFO buffer library

#define AP_INTERTIALNAV_TC_Z    0.2f // default time constant for complementary filter's Z axis

class AP_ComplFilter {
public:
    AP_ComplFilter(const AP_AHRS *ahrs, const RangeFinder &rng);

    /**
     * update - updates velocity and position estimates using latest info from accelerometers
     * augmented with rngfnd readings
     *
     * @param dt : time since last update in seconds
     */
    void update(float dt);

    float get_altitude() { return _position; }

private:

   /**
     * check_rngfnd - checks if new rngfnd readings have arrived and calls correct_with_rngfnd to
     * calculate the vertical position error
     *
     * @see correct_with_rngfnd(float rngfnd, float dt);
     */
    void check_rngfnd();

    /**
     * correct_with_rngfnd - calculates vertical position error using rngfnd.
     *
     * @param rngfnd : rngfnd in cm
     */
    void correct_with_rngfnd(float rngfnd);

    /**
     * set_altitude - overwrites the current altitude value.
     *
     * @param new_altitude : altitude in cm
     */
    void set_altitude( float new_altitude);
    
    const AP_AHRS *_ahrs;
    const RangeFinder &_rng;

    float                   _k1_z;                      // gain for vertical position correction
    float                   _k2_z;                      // gain for vertical velocity correction
    float                   _k3_z;                      // gain for vertical accelerometer offset correction
    AP_BufferFloat_Size8   _hist_position_estimate_z;  // buffer of historic accel based altitudes to account for rngfnd lag

    float                accel_correction_ef;
    float                _position_base;             // (uncorrected) position estimate in cm
    float                _position_correction;       // sum of corrections to _position_base from delayed 1st order samples in cm
    float                _velocity;                  // latest velocity estimate (integrated from accelerometer values) in cm/s
    float                _position_error;            // current position error in cm - is set by the check_* methods and used by update method to calculate the correction terms
    float                _position;                  // sum(_position_base, _position_correction) - corrected position estimate in cm
};

